/*
 * Copyright 2021 <copyright holder> <email>
 * 
 * Licensed under the Apache License, Version 2.0 (the "License");
 * you may not use this file except in compliance with the License.
 * You may obtain a copy of the License at
 * 
 *     http://www.apache.org/licenses/LICENSE-2.0
 * 
 * Unless required by applicable law or agreed to in writing, software
 * distributed under the License is distributed on an "AS IS" BASIS,
 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
 * See the License for the specific language governing permissions and
 * limitations under the License.
 */

#include "camera.h"

camera::camera(float cf, float ca, cv::Size size, float* dist, float* pos, float* post)
{
    f=cf;
    
    alph=ca;
    
    imagesize=size;
    planesize=imagesize;
    
    for(int i=0;i<3;i++){    
        currentposture.pos[i]=0.0;
        beginposture.pos[i]=*(pos+i);
    }
    
    for(int i=0;i<5;i++){
        distor[i]=*(dist+i);
    }
    
    for(int i=0;i<3;i++){
        for(int j=0;j<3;j++){
            beginposture.post[i][j]=*(post+i*3+j);
            currentposture.post[i][j]=0.0;
        }
    }
    currentposture.post[0][0]=1;
    currentposture.post[1][1]=1;
    currentposture.post[2][2]=1;
    
    camerapath.push_back(currentposture);
}


cv::Mat camera::getinstrinsic()
{
    cv::Mat K=(cv::Mat_<double>(3,3)<<f,0,float(imagesize.width/2.0),0,f*alph,float(imagesize.height/2.0),0,0,1);
    return K;
}

cv::Mat camera::getdistcoefficient()
{
    cv::Mat D=(cv::Mat(1,5,CV_64F,distor));
    return D;
}

bool camera::updatepost(const posture &newposture)
{
    currentposture=newposture;
    updatecamerapath();
}

cv::Mat camera::getpos()
{
    updatecamerapath();
    posture post=camerapath.front();
    cv::Mat pos=(cv::Mat_<double>(1,3)<<post.pos[0],post.pos[1],post.pos[2]);
    return pos;
}

bool camera::updatecamerapath()
{
    camerapath.push_back(currentposture);   
}

std::vector<posture> camera::getcamerapath()
{
    return camerapath;
}


cv::Mat camera::getw2c()
{
    cv::Mat R=(cv::Mat_<double>(3,3)<<beginposture.post[0][0],beginposture.post[0][1],beginposture.post[0][2],
                                    beginposture.post[1][0],beginposture.post[1][1],beginposture.post[1][2],
                                    beginposture.post[2][0],beginposture.post[2][1],beginposture.post[2][2]);
    cv::Mat t=(cv::Mat_<double>(3,1)<<beginposture.pos[0],beginposture.pos[1],beginposture.pos[2]);
     cv::Mat invR=cv::Mat(3,3,CV_64F);
    cv::invert(R, invR, cv::DECOMP_LU);
    cv::Mat pt=invR*t;
    cv::Mat rt=(cv::Mat_<double>(3,1)<<-pt.at<double>(0,0),-pt.at<double>(0,1),-pt.at<double>(0,2));
    cv::Mat transfer =cv::Mat(3,4,CV_64F);
    hconcat(R,rt,transfer); 
    return transfer;
}


cv::Point2f camera::project(const cv::Point3f &pworld)
{
    cv::Mat trans=getw2c();
    
    
    cv::Mat wpoint=(cv::Mat_<double>(4,1)<<pworld.x,pworld.y,pworld.z,1.0);
    
    cv::Mat cpoint4=trans*wpoint;
    
    
    cv::Mat cpoint3=
            (cv::Mat_<double>(3,1)<<cpoint4.at<double>(0,0),cpoint4.at<double>(1,0),cpoint4.at<double>(2,0));
         
    cv::Mat ppoint2=(cv::Mat_<double>(2,1)<<cpoint3.at<double>(0,0)/cpoint3.at<double>(2,0),
                     cpoint3.at<double>(1,0)/cpoint3.at<double>(2,0));
     
     
    double r2=pow(ppoint2.at<double>(0,0),2)+pow(ppoint2.at<double>(1,0),2);
 
    cv::Mat dispoint2=cv::Mat(2,1,CV_64F);

    dispoint2.at<double>(0,0)=ppoint2.at<double>(0,0)*(1+distor[0]*r2+distor[1]*pow(r2,2))
                            +2*distor[3]*ppoint2.at<double>(0,0)*ppoint2.at<double>(1,0)
                            +distor[4]*(pow(ppoint2.at<double>(0,0),2)+pow(r2,2));
    std::cout<<dispoint2.at<double>(0,0)<<std::endl;
                            
    dispoint2.at<double>(1,0)=ppoint2.at<double>(1,0)*(1+distor[0]*r2+distor[1]*pow(r2,2))
                            +2*distor[4]*ppoint2.at<double>(0,0)*ppoint2.at<double>(1,0)
                            +distor[3]*(pow(ppoint2.at<double>(0,0),2)+pow(r2,2));
                            
    cv::Mat pixelpoint2=cv::Mat(2,1,CV_64F);
    pixelpoint2.at<double>(0,0)=dispoint2.at<double>(0,0)*
                                (imagesize.width/planesize.width)+imagesize.width/2;
    pixelpoint2.at<double>(1,0)=dispoint2.at<double>(1,0)*
                                (imagesize.height/planesize.height)+imagesize.height/2;
    
    cv::Point2f pixel;
    pixel.x=pixelpoint2.at<double>(0,0);
    pixel.y=pixelpoint2.at<double>(1,0);
    
    if(-1<pixel.x&&pixel.x<imagesize.width+1&&-1<pixel.y&&pixel.y<imagesize.height+1){
        return pixel;
    }
    else{
        std::cout<<"a wrong pixel"<<std::endl;
        return pixel;
    }
}


















































